// --------------------------------------------------------------------------------------------------------------------
// <copyright file="SlamMapperDriveAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Navigation
{
    using System;
    using System.Runtime.Serialization;
    using System.Threading.Tasks;

    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Hardware.Runtime.DAC;
    using Microsoft.Robotics.Interaction.Runtime.Input;
    using Microsoft.Robotics.Navigation.Motion;
    using Microsoft.Robotics.Navigation.Runtime.Motion;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Vision.Cameras;
    using Microsoft.Robotics.Vision.Runtime.Cameras;

    /// <summary>
    /// Implements SLAM mode drive with gamepad override. We will keep this in the test hierarchy until we understand requirements
    /// on this component better by using this class and create a proper production class.
    /// </summary>
    [DataContract, System.Runtime.InteropServices.GuidAttribute("0408D3D9-1021-4B0F-BCEB-86C1D7C22017")]
    public class SlamMapperDriveAgent : ConsumerProducerAgent<GamePadAgentMessage, MobileBaseVelocityReferenceMessage>,
        ISubscriptionReceiver<SimpleAgentMessage<DriveFeedbackState>>, // need to know the distance traveled
        ISubscriptionReceiver<DepthCameraAgentMessage<KinectDepthCameraParameters>>, // 'raw' Kinect frame in
        ISubscriptionReceiver<JointMessage>, // need this to know turret rotation speed and determine stable positions
        IMessagePublisher<DepthCameraAgentMessage<KinectDepthCameraParameters>> // 'civilized' Kinect frames are sent out when robot is stable and frames can be used for mapping
    {
        /// <summary>
        /// Max drive max speed
        /// </summary>
        [DataMember(Name = "DriveSpeedMax")]
        private double? driveSpeedMax;

        /// <summary>
        /// Max rotation max speed
        /// </summary>
        [DataMember(Name = "DriveRotationMax")]
        private double? driveRotationMax;

        /// <summary>
        /// Name of the drive feedback agent
        /// </summary>
        [DataMember(Name = "DriveFeedbackAgent")]
        private string driveFeedbackAgentName = default(string);

        /// <summary>
        /// The name of the pan/tilt joint position producer we subscribe to.
        /// </summary>
        [DataMember(Name = "JointMessageProducer")]
        private string jointMessageProducer = default(string);

        /// <summary>
        /// Name of the heartbeat agent
        /// </summary>
        [DataMember(Name = "HeartbeatAgent")]
        private string heartbeatAgentName = default(string);

        /// <summary>
        /// Name of the Kinect message producer agent
        /// </summary>
        [DataMember(Name = "KinectAgent")]
        private string kinectAgentName = default(string);

        /// <summary>
        /// Interval in meters between stopping and scanning in autonomous scan mode
        /// </summary>
        [DataMember(Name = "StopForCaptureIntervalInMeters")]
        private double? stopForCaptureIntervalInMeters;

        /// <summary>
        /// Size of a single step during turret scan.
        /// </summary>
        [DataMember(Name = "TurretSweepStepsInRadians")]
        private double? turretSweepStepsInRadians;

        /// <summary>
        /// Minimal angle in radians from where to start sweeps
        /// </summary>
        [DataMember(Name = "TurretSweepRangeInRadiansStart")]
        private double? turretSweepRangeInRadiansStart;

        /// <summary>
        /// Maximum angle in radians where to stop sweeps
        /// </summary>
        [DataMember(Name = "TurretSweepRangeInRadiansEnd")]
        private double? turretSweepRangeInRadiansEnd;

        /// <summary>
        /// Turret rotation speed below which we'll consider Kinect as 'stable' and re-transmit
        /// </summary>
        [DataMember(Name = "ConsiderTurretStableIfRotatingSlowerThanThisInRadsPerSecond")]
        private double? considerTurretStableIfRotatingSlowerThanThis;

        /// <summary>
        /// Delay between initiating sweeps
        /// </summary>
        [DataMember(Name = "TurretHoldPositionInMilliseconds")]
        private int? turretHoldPositionInMilliseconds;

        /// <summary>
        /// Position between TurretSweepPositionStart and TurretSweepPositionEnd where turret is rested while driving forward
        /// </summary>
        [DataMember(Name = "TurretDefaultHeading")]
        private double? turretDefaultHeading;

        /// <summary>
        /// Desired Turret rotation speed in radians per second
        /// </summary>
        [DataMember(Name = "DesiredTurretPanRotationSpeed")]
        private double desiredTurretPanRotationSpeed;

        /// <summary>
        /// Turret pan message forwarder.
        /// </summary>
        private IForwarder<SetServoSequenceMessage> panForwarder;

        /// <summary>
        /// Stable Kinect Message forwarder (will get messages only when robot is stable)
        /// </summary>
        private IForwarder<DepthCameraAgentMessage<KinectDepthCameraParameters>> stableKinectPublisher;

        /// <summary>
        /// Current pan position of the turret
        /// </summary>
        private double turretDesiredPanAngle;

        /// <summary>
        /// Previous drive feedback state at which capture happened.
        /// </summary>
        private DriveFeedbackState lastDriveFeedbackState;

        /// <summary>
        /// Joystick-settable flag indicating whether to keep making regular stops, moving turret to take captures
        /// </summary>
        private bool allowAutomaticTurretScans;

        /// <summary>
        /// Automatically-settable flag indicating whether robot is currently in capture mode (will result in 
        /// temporary drive suspension)
        /// </summary>
        private bool turretRotationInProgress;

        /// <summary>
        /// Current Turret rotation speed in radians per second
        /// </summary>
        private double currentTurretPanRotationSpeed;

        /// <summary>
        /// Current turret pan angle in radians.
        /// </summary>
        private volatile float turretCurrentPanAngle;

        /// <summary>
        /// If not zero, the robot drives full speed forward. Counts the number of iterations with trigger not pressed and shuts down the drive when it reaches 0.
        /// </summary>
        private int continuousDriveShutdownCounter;

        /// <summary>
        /// Initializes a new instance of the SlamMapperDriveAgent class
        /// </summary>
        /// <param name="name">The agent name</param>
        /// <param name="producer">The producer name</param>
        public SlamMapperDriveAgent(string name, string producer)
            : base(name, producer)
        {
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="SlamMapperDriveAgent"/> class.
        /// </summary>
        /// <param name="name">Agent name</param>
        /// <param name="gamePadProducer">The game pad producer</param>
        /// <param name="heartbeatAgentName">Name of the heartbeat agent</param>
        /// <param name="kinectAgentName">Name of the Kinect message producer agent</param>
        /// <param name="driveFeedbackAgentName">Name of the drive feedback producer agent</param>
        /// <param name="stopForCaptureIntervalInMeters">Robot will stop every this many meters and scan with the turret</param>
        /// <param name="turretSweepStepsInRadians">Turret Sweep Step In Radians</param>
        /// <param name="turretHoldPositionInMilliseconds">Turret Hold Position In Milliseconds</param>
        /// <param name="turretSweepRangeInRadiansStart">Turret Sweep Range In Radians Start</param>
        /// <param name="turretSweepRangeInRadiansEnd">Turret Sweep Range In Radians End</param>
        /// <param name="turretDefaultHeading">Turret Default Heading</param>
        /// <param name="driveSpeedMax">Drive speed max</param>
        /// <param name="driveRotationMax">Drive rotation max</param>        
        /// <param name="jointMessageProducer">Joint Message Producer</param>
        /// <param name="considerTurretStableIfRotatingSlowerThanThis">Consider Turret Stable If Rotating Slower Than This</param>
        /// <param name="robotConfigFolder">Robot config folder</param>
        /// <param name="desiredTurretPanRotationSpeed">Desired turret pan rotation speed</param>
        public SlamMapperDriveAgent(
            string name,
            string gamePadProducer,
            string heartbeatAgentName,
            string kinectAgentName,
            string driveFeedbackAgentName,
            double stopForCaptureIntervalInMeters,
            double turretSweepStepsInRadians,
            int turretHoldPositionInMilliseconds,
            double turretSweepRangeInRadiansStart,
            double turretSweepRangeInRadiansEnd,
            double turretDefaultHeading,
            double driveSpeedMax,
            double driveRotationMax,
            string jointMessageProducer,
            double considerTurretStableIfRotatingSlowerThanThis,
            string robotConfigFolder,
            double desiredTurretPanRotationSpeed)
            : base(name, gamePadProducer)
        {
            this.heartbeatAgentName = heartbeatAgentName;
            this.kinectAgentName = kinectAgentName;
            this.driveFeedbackAgentName = driveFeedbackAgentName;
            this.jointMessageProducer = jointMessageProducer;
            this.stopForCaptureIntervalInMeters = stopForCaptureIntervalInMeters;
            this.turretSweepStepsInRadians = turretSweepStepsInRadians;
            this.turretHoldPositionInMilliseconds = turretHoldPositionInMilliseconds;
            this.turretSweepRangeInRadiansStart = turretSweepRangeInRadiansStart;
            this.turretSweepRangeInRadiansEnd = turretSweepRangeInRadiansEnd;
            this.turretDefaultHeading = turretDefaultHeading;
            this.driveSpeedMax = driveSpeedMax;
            this.driveRotationMax = driveRotationMax;
            this.considerTurretStableIfRotatingSlowerThanThis = considerTurretStableIfRotatingSlowerThanThis;
            this.desiredTurretPanRotationSpeed = desiredTurretPanRotationSpeed;
        }

        /// <summary>
        /// Called on agent initialization
        /// </summary>
        /// <param name="locator">The agent locator.</param>
        public override void Initialize(AgentLocator locator)
        {
            if (this.heartbeatAgentName == default(string) ||
                this.kinectAgentName == default(string) ||
                this.driveFeedbackAgentName == default(string) ||
                this.jointMessageProducer == default(string) ||
                this.turretSweepRangeInRadiansEnd.Value - this.turretSweepRangeInRadiansStart.Value <= 0 ||
                this.turretSweepStepsInRadians > ((this.turretSweepRangeInRadiansEnd.Value - this.turretSweepRangeInRadiansStart.Value)) ||
                this.turretDefaultHeading.Value > this.turretSweepRangeInRadiansEnd.Value ||
                this.turretDefaultHeading.Value < this.turretSweepRangeInRadiansStart.Value ||
                !this.turretHoldPositionInMilliseconds.HasValue ||
                this.turretHoldPositionInMilliseconds.Value < 1 ||
                !this.driveSpeedMax.HasValue ||
                !this.driveRotationMax.HasValue ||
                !this.stopForCaptureIntervalInMeters.HasValue ||
                !this.considerTurretStableIfRotatingSlowerThanThis.HasValue ||
                this.desiredTurretPanRotationSpeed == 0)
            {
                throw new ArgumentException(string.Format("At least one required initialization parameter is missing for {0}.", this.GetType().FullName));
            }

            this.turretDesiredPanAngle = 0;
            this.lastDriveFeedbackState = null;
            this.allowAutomaticTurretScans = false;
            this.turretRotationInProgress = false;

            IAgentHost heartbeatAgent = locator.GetAgent(this.heartbeatAgentName);
            this.panForwarder = heartbeatAgent.GetControlPort<SetServoSequenceMessage>();

            base.Initialize(locator);

            locator.GetAgent(this.kinectAgentName).Subscribe((ISubscriptionReceiver<DepthCameraAgentMessage<KinectDepthCameraParameters>>)this);

            locator.GetAgent(this.driveFeedbackAgentName).Subscribe((ISubscriptionReceiver<SimpleAgentMessage<DriveFeedbackState>>)this);

            locator.GetAgent(this.jointMessageProducer).Subscribe((ISubscriptionReceiver<JointMessage>)this);
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<DepthCameraAgentMessage<KinectDepthCameraParameters>> forwarder)
        {
            this.stableKinectPublisher = forwarder;
        }

        /// <summary>
        /// Called when the agent is activated
        /// </summary>
        public override void OnActivated()
        {
            base.OnActivated();
        }

        /// <summary>
        /// Handle Kinect agent message, filter out frames that are received in a non-stable mode (i.e. when robot is moving)
        /// </summary>
        /// <param name="message">Message with Kinect frame</param>
        public void Receive(DepthCameraAgentMessage<KinectDepthCameraParameters> message)
        {
            if (this.currentTurretPanRotationSpeed < this.considerTurretStableIfRotatingSlowerThanThis)
            {
                this.stableKinectPublisher.Post(message);
            }
        }

        /// <summary>
        /// We need to process distance information to initiate turret scanning
        /// </summary>
        /// <param name="message">Message with drive state</param>
        public void Receive(SimpleAgentMessage<DriveFeedbackState> message)
        {
            if (this.allowAutomaticTurretScans)
            {
                if (this.lastDriveFeedbackState == null)
                {
                    this.lastDriveFeedbackState = message.State;
                    return;
                }

                Pose2D deltaPose = message.State.GetMeanDeltaPose(this.lastDriveFeedbackState);

                if (Math.Abs(deltaPose.X) > this.stopForCaptureIntervalInMeters ||
                    Math.Abs(deltaPose.X) > this.stopForCaptureIntervalInMeters)
                {
                    this.lastDriveFeedbackState = message.State;

                    this.InitiateTurretScan(message);
                }
            }
        }

        /// <summary>
        /// We need to process joint message to determine slow speed at which we'll retransmit stable Kinect frames 
        /// </summary>
        /// <param name="jointMsg">Turret Joint state</param>
        public void Receive(JointMessage jointMsg)
        {
            const int PanIndex = 1;

            this.currentTurretPanRotationSpeed = jointMsg.State[PanIndex].CurrentVelocityInRadiansPerSecond;
            this.turretCurrentPanAngle = (float)jointMsg.State[PanIndex].PositionInRadians;
        }

        /// <summary>
        /// Handle game pad messages
        /// </summary>
        /// <param name="message">Message with game pad state</param>
        public override void Receive(GamePadAgentMessage message)
        {
            if (message.Buttons.X)
            {
                this.InitiateTurretScan(message);
            }

            this.ControlRobotMotion(message);
        }

        /// <summary>
        /// Manually control turret angle
        /// </summary>
        /// <param name="message">Gamepad Message</param>
        private void ControlTurretAngle(GamePadAgentMessage message)
        {
            if (!this.turretRotationInProgress)
            {
                if (message.DPad.Right)
                {
                    if (this.turretDesiredPanAngle + this.turretSweepStepsInRadians <= this.turretSweepRangeInRadiansEnd)
                    {
                        this.SetTurretHeading(this.turretDesiredPanAngle + this.turretSweepStepsInRadians.Value, message);
                    }
                }

                if (message.DPad.Left)
                {
                    if (this.turretDesiredPanAngle - this.turretSweepStepsInRadians >= this.turretSweepRangeInRadiansStart.Value)
                    {
                        this.SetTurretHeading(this.turretDesiredPanAngle - this.turretSweepStepsInRadians.Value, message);
                    }
                }
            }
        }

        /// <summary>
        /// Drive robot using left thumb stick.
        /// Note! Game pad control logic (with regards to driving) will have to be unified between various drive agents we have. (TFS item 1264)
        /// </summary>
        /// <param name="message">Gamepad Message</param>
        private void ControlRobotMotion(GamePadAgentMessage message)
        {
            if (!this.turretRotationInProgress)
            {
                const double MinMeaningfulAnalogControlValue = 0.3;

                float xThumb = message.ThumbSticks.Right.X;
                float yThumb = message.ThumbSticks.Left.Y;

                int boost = (message.Triggers.Left > 0) ? 3 : 1;

                if (message.Triggers.Right > 0)
                {
                    this.continuousDriveShutdownCounter = 25;
                }
                else
                {
                    this.continuousDriveShutdownCounter--;
                }

                if (Math.Abs(xThumb) < MinMeaningfulAnalogControlValue)
                {
                    xThumb = 0;
                }

                if (Math.Abs(yThumb) < MinMeaningfulAnalogControlValue)
                {
                    yThumb = this.continuousDriveShutdownCounter > 0 ? 1 : 0;
                }
                else
                {
                    this.continuousDriveShutdownCounter = 0;
                }

                double targetXVelocity = this.driveSpeedMax.Value * yThumb * boost;
                double targetTurnVelocity = -this.driveRotationMax.Value * xThumb * boost;

                MobileBaseVelocityReference velocityUpdate = new MobileBaseVelocityReference(targetXVelocity, 0, targetTurnVelocity);

                MobileBaseVelocityReferenceMessage newMessage = new MobileBaseVelocityReferenceMessage(velocityUpdate, message);

                this.Publisher.Post(newMessage);
            }
        }

        /// <summary>
        /// Sends the turret position message
        /// </summary>
        /// <param name="heading">Turret target heading</param>
        /// <param name="initiatingMessage">Initiating message</param>
        /// <param name="duration">Duration of turn</param>
        private void SetTurretHeading(double heading, AgentMessage initiatingMessage, uint duration = 0)
        {
            if (heading < this.turretSweepRangeInRadiansStart.Value)
            {
                heading = this.turretSweepRangeInRadiansStart.Value;
            }
            else if (heading > this.turretSweepRangeInRadiansEnd.Value)
            {
                heading = this.turretSweepRangeInRadiansEnd.Value;
            }

            this.turretDesiredPanAngle = heading;

            this.panForwarder.Post(new SetServoSequenceMessage((float)this.turretDesiredPanAngle, duration, initiatingMessage));
            Task.Delay((int)(duration)).Wait();
        }

        /// <summary>
        /// Initiates turret scan
        /// </summary>
        /// <param name="initiatingMessage">Initiating message</param>
        private async void InitiateTurretScan(AgentMessage initiatingMessage)
        {
            // this is not really intended as a fail-safe synchronization, but more as a sanity defense against
            // multiple user-initiated scans or to prevent user from resetting scan by accidentally issuing a manual sweep
            // while an autonomous one is in progress
            if (this.turretRotationInProgress)
            {
                return;
            }

            this.turretRotationInProgress = true;

            // We need to stop the robot before starting turret sweeps
            MobileBaseVelocityReference velocityUpdate = new MobileBaseVelocityReference(0, 0, 0);
            MobileBaseVelocityReferenceMessage newMessage = new MobileBaseVelocityReferenceMessage(velocityUpdate, initiatingMessage);
            this.Publisher.Post(newMessage);
            uint duration = (uint)((Math.Abs(0 - this.turretCurrentPanAngle) / this.desiredTurretPanRotationSpeed) * MathConstants.MilliunitsPerUnit);
            this.SetTurretHeading(0, initiatingMessage, duration);
            await this.SmoothScanSection(this.turretDefaultHeading.Value, this.turretSweepRangeInRadiansStart.Value, initiatingMessage, false);
            await this.SmoothScanSection(this.turretSweepRangeInRadiansStart.Value, this.turretSweepRangeInRadiansEnd.Value, initiatingMessage, true);
            await this.SmoothScanSection(this.turretSweepRangeInRadiansEnd.Value, this.turretDefaultHeading.Value, initiatingMessage, false);

            this.turretRotationInProgress = false;
        }

        /// <summary>
        /// Sweeps turret from a given start position to the end position
        /// </summary>
        /// <param name="startPosition">Start pan position in radians</param>
        /// <param name="endPostion">End pan position in radians</param>
        /// <param name="initiatingMessage">Initiating message</param>
        /// <param name="useSteps">Use steps while panning</param>
        /// <returns>Wait-able task</returns>        
        private async Task SmoothScanSection(double startPosition, double endPostion, AgentMessage initiatingMessage, bool useSteps)
        {
            int iterations = 0;
            if (useSteps)
            {
                iterations = (int)(Math.Abs(((endPostion - startPosition) / this.turretSweepStepsInRadians.Value)));
            }

            double curPosition = startPosition;

            // we go one extra step to make sure we cover the 100% of the section in cases where section is not evenly divisible
            // by step sizes
            for (int i = 0; i < iterations + 1; ++i)
            {
                // do it here for realtime update if this.turretSweepStepsInRadians is changed by user in realtime
                double stepDirection = startPosition < endPostion ? 1 : -1;
                double stepVal = Math.Min(Math.Abs(endPostion - startPosition), Math.Abs(this.turretSweepStepsInRadians.Value));
                double singlePanStep = stepVal * stepDirection;

                curPosition += singlePanStep;

                if (iterations == i)
                {
                    curPosition = endPostion;
                }

                uint duration = (uint)((Math.Abs(curPosition - this.turretCurrentPanAngle) / this.desiredTurretPanRotationSpeed) * MathConstants.MilliunitsPerUnit);
                this.SetTurretHeading(curPosition, initiatingMessage, duration);
                await Task.Delay((int)(this.turretHoldPositionInMilliseconds)); // let robot and turret settle
            }
        }
    }
}
